/*
 * SsrrController.cpp
 *
 *  Created on: Oct 23, 2013
 *      Author: incubed
 */

#include <ros/ros.h>
#include <ssrr_ompl_planner/SsrrController.h>
#include <vector>

namespace ssrr {

#define MAX_COST 1e9;


SsrrController::SsrrController(double min_trans, double max_trans, double escape_trans, double min_rot, double max_rot) {
	min_trans_ = min_trans;
	max_trans_ = max_trans;
	escape_trans_ = escape_trans;
	min_rot_ = min_rot;
	max_rot_ = max_rot;
}

SsrrController::~SsrrController() {

}

bool SsrrController::control(double pos_x, double pos_y, double pos_th, double cur_trans, double cur_rot, double &result_trans, double &result_rot, geometry_msgs::PoseStamped& goal, nav_msgs::Path &path) {
	double test_trans = min_trans_;
	double step_trans = (max_trans_ - min_trans_) / 10.;
	double test_rot = min_rot_;
	double step_rot = (max_rot_ - min_rot_) / 10.;
	double sim_time = 1.0;
	double sim_step = 0.05;
	double min_cost = MAX_COST;
	std::vector<Trajectory> trajectories;
	Trajectory* best = NULL;
	result_trans = 0;
	result_rot = 0;
	for (double test_trans = min_trans_; test_trans <= max_trans_; test_trans += step_trans) {
		for (double test_rot = min_rot_; test_rot <= max_rot_; test_rot += step_rot) {
			trajectories.push_back(createTrajectory(pos_x, pos_y, pos_th, test_trans, test_rot, sim_time, sim_step));
			Trajectory &traj = trajectories.back();
			double final_x;
			double final_y;
			double final_th;
			traj.getEndpoint(final_x, final_y, final_th);
			double goal_cost = getGoalCost(final_x, final_y, final_th, goal);
			traj.setGoalCost(goal_cost);
			double path_cost = getPathCost(final_x, final_y, final_th, path);
			traj.setPathCost(path_cost);
			if (traj.getCost() <= min_cost) {
				min_cost = traj.getCost();

				best = traj;
			}

		}
	}
	if (best != NULL && best->getCost() > 0) {
		ROS_INFO("found controller command %.3f %.3f gc: %.3f pc %.3f oc: %.3f", best->xv_, best->thetav_, best->getGoalCost(), best->getPathCost(), best->getObstacleCost());
		result_trans = best->xv_;
		result_rot = best->thetav_;
		return true;
	}
	else {
		ROS_INFO("no controller command");
	}
	return false;
}

Trajectory SsrrController::createTrajectory(double x, double y, double th, double trans, double rot, double sim_time, double sim_step) {
	Trajectory trajectory;
	bool init = false;
	double last_x = x;
	double last_y = y;
	double last_th = th;
	for (double time = 0; time <= sim_time; time += sim_step) {
		trajectory.addPoint(x, y, th);
		x += sim_step * (trans * cos(rot));
		y += sim_step * (trans * sin(rot));
		th += sim_step * rot;
		if (init) {
			double obstacle_cost = getObstacleCost(last_x, last_y, last_th, x, y, th);
			trajectory.updateObstacleCost(obstacle_cost);
		}
		last_x = x;
		last_y = y;
		last_th = th;
		init = true;
	}

	return trajectory;
}

double SsrrController::getGoalCost(double x, double y, double th, geometry_msgs::PoseStamped &pose) {
	return dist(x, y, pose.pose.position.x, pose.pose.position.y);
}

double SsrrController::getPathCost(double x, double y, double th, nav_msgs::Path &path) {
	uint size = path.poses.size();
	double min_dist = MAX_COST;
	for (uint index = 0; index < size; index++) {
		geometry_msgs::PoseStamped &pose = path.poses[index];
		double distance = dist(x, y, pose.pose.position.x, pose.pose.position.y);
		if (distance < min_dist) {
			min_dist = distance;
		}
	}
	return min_dist;
}

double SsrrController::getObstacleCost(double last_x, double last_y, double last_th, double x, double y, double th) {
	return 1.0;
}


